#include "robot_chassis/chassis_control.hpp"
#include <iostream>
#include <iomanip>
#include <csignal>

ChassisController::ChassisController() 
    : Node("chassis_controller"),
      running(true),
      max_linear_x_(0.1),
      max_linear_y_(0.1),
      max_angular_z_(0.3),
      accel_linear_(0.5),
      accel_angular_(1.0),
      decel_linear_(1.5),  // 减速度是加速度的3倍，使速度归零更快
      decel_angular_(3.0),
      target_linear_x_(0.0),
      target_linear_y_(0.0),
      target_angular_z_(0.0),
      max_target_linear_x_(0.0),
      max_target_linear_y_(0.0),
      max_target_angular_z_(0.0),
      current_linear_x_(0.0),
      current_linear_y_(0.0),
      current_angular_z_(0.0),
      prev_current_linear_x_(0.0),
      prev_current_linear_y_(0.0),
      prev_current_angular_z_(0.0),
      control_rate_(100.0),
      dt_(1.0 / 50.0),
      last_target_linear_x_(0.0),
      last_target_linear_y_(0.0),
      last_target_angular_z_(0.0),
      last_current_linear_x_(0.0),
      last_current_linear_y_(0.0),
      last_current_angular_z_(0.0),
      first_print_(true),
      key_timeout_(0.15),
      stdin_fd_(STDIN_FILENO),
      last_key_press_time_(),
      key_pressed_(false),
      last_processed_key_(KeyType::NONE),
      last_processed_key_time_(),
      key_debounce_time_(0.05) {  // 50ms防抖时间
    
    // 设置日志级别为FATAL，减少日志输出
    this->get_logger().set_level(rclcpp::Logger::Level::Fatal);
    
    // 创建发布者
    publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("chassis/cmd_vel", 10);
    
    // 保存原始终端设置
    tcgetattr(stdin_fd_, &original_settings_);
    
    // 设置终端为原始模式
    struct termios raw_settings = original_settings_;
    cfmakeraw(&raw_settings);
    tcsetattr(stdin_fd_, TCSANOW, &raw_settings);
    
    // 设置stdin为非阻塞
    int flags = fcntl(stdin_fd_, F_GETFL, 0);
    fcntl(stdin_fd_, F_SETFL, flags | O_NONBLOCK);
    
    // 创建定时器
    timer_ = this->create_wall_timer(
        std::chrono::milliseconds(static_cast<int>(1000.0 / control_rate_)),
        std::bind(&ChassisController::updateVelocity, this));
    
    // 打印初始化信息
    printInitInfo();
}

ChassisController::~ChassisController() {
    // 恢复终端设置
    tcsetattr(stdin_fd_, TCSADRAIN, &original_settings_);
}

void ChassisController::printInitInfo() {
    // 清除屏幕并移动到顶部
    std::cout << "\033[2J\033[H";
    std::cout << std::string(70, '=') << "\r\n";
    std::cout << "麦克纳姆轮渐进式速度控制\r\n";
    std::cout << std::string(70, '=') << "\r\n";
    std::cout << "控制键说明:\r\n";
    std::cout << "  方向上键 : vx 增加到最高速度（前进）\r\n";
    std::cout << "  方向下键 : vx 减少到最高速度的负数（后退）\r\n";
    std::cout << "  方向左键 : 车左转（角速度）\r\n";
    std::cout << "  方向右键 : 车右转（角速度）\r\n";
    std::cout << "  w        : 增加 x 轴向的最大速度\r\n";
    std::cout << "  s        : 减小 x 轴向的最大速度\r\n";
    std::cout << "  a        : 增加旋转最高速度\r\n";
    std::cout << "  d        : 减小旋转最高速度\r\n";
    std::cout << "  q        : 增加 y 轴向最高速度\r\n";
    std::cout << "  e        : 减小 y 轴向最高速度\r\n";
    std::cout << "  Space/x  : 立即停止所有速度（直接置零）\r\n";
    std::cout << "  Ctrl+C   : 退出程序\r\n";
    std::cout << std::string(70, '=') << "\r\n";
    std::cout.flush();
    
    // 初始化时显示两行速度信息（显示最高速度）
    std::cout << std::fixed << std::setprecision(3);
    std::cout << "最高速度: vx=" << max_linear_x_ 
              << " m/s, vy=" << max_linear_y_ 
              << " m/s, wz=" << max_angular_z_ << " rad/s\r\n";
    std::cout << "当前速度: vx=" << current_linear_x_ 
              << " m/s, vy=" << current_linear_y_ 
              << " m/s, wz=" << current_angular_z_ << " rad/s";
    std::cout.flush();
    
    // 初始化显示记录值
    last_target_linear_x_ = max_linear_x_;
    last_target_linear_y_ = max_linear_y_;
    last_target_angular_z_ = max_angular_z_;
    
    first_print_ = false;
}

void ChassisController::printStatus() {
    // 检查是否有变化（使用最大速度变量，而不是max_target）
    bool max_speed_changed = (std::abs(max_linear_x_ - last_target_linear_x_) > 0.001 ||
                              std::abs(max_linear_y_ - last_target_linear_y_) > 0.001 ||
                              std::abs(max_angular_z_ - last_target_angular_z_) > 0.001);
    
    bool current_changed = (std::abs(current_linear_x_ - last_current_linear_x_) > 0.001 ||
                           std::abs(current_linear_y_ - last_current_linear_y_) > 0.001 ||
                           std::abs(current_angular_z_ - last_current_angular_z_) > 0.001);
    
    if (!max_speed_changed && !current_changed) {
        return;
    }
    
    std::cout << std::fixed << std::setprecision(3);
    
    if (first_print_) {
        std::cout << "最高速度: vx=" << max_linear_x_ 
                  << " m/s, vy=" << max_linear_y_ 
                  << " m/s, wz=" << max_angular_z_ << " rad/s\r\n";
        std::cout << "当前速度: vx=" << current_linear_x_ 
                  << " m/s, vy=" << current_linear_y_ 
                  << " m/s, wz=" << current_angular_z_ << " rad/s";
        first_print_ = false;
    } else {
        // 向上移动1行，更新最高速度行
        std::cout << "\033[1A\r\033[K";
        std::cout << "最高速度: vx=" << max_linear_x_ 
                  << " m/s, vy=" << max_linear_y_ 
                  << " m/s, wz=" << max_angular_z_ << " rad/s\r\n";
        std::cout << "\033[K";
        std::cout << "当前速度: vx=" << current_linear_x_ 
                  << " m/s, vy=" << current_linear_y_ 
                  << " m/s, wz=" << current_angular_z_ << " rad/s";
    }
    std::cout.flush();
    
    // 更新记录的值
    last_target_linear_x_ = max_linear_x_;
    last_target_linear_y_ = max_linear_y_;
    last_target_angular_z_ = max_angular_z_;
    last_current_linear_x_ = current_linear_x_;
    last_current_linear_y_ = current_linear_y_;
    last_current_angular_z_ = current_angular_z_;
}

void ChassisController::updateVelocity() {
    // 检查按键超时 - 简化且更可靠的检测逻辑
    auto current_time = std::chrono::steady_clock::now();
    if (key_pressed_ && last_key_press_time_ != std::chrono::steady_clock::time_point{}) {
        auto time_since_last_key = std::chrono::duration<double>(
            current_time - last_key_press_time_).count();
        
        if (time_since_last_key > key_timeout_) {
            // 检查当前速度是否已经达到目标速度（在允许误差范围内）
            bool reached_target_x = std::abs(current_linear_x_ - target_linear_x_) < 0.01;
            bool reached_target_y = std::abs(current_linear_y_ - target_linear_y_) < 0.01;
            bool reached_target_z = std::abs(current_angular_z_ - target_angular_z_) < 0.01;
            bool reached_target = reached_target_x && reached_target_y && reached_target_z;
            
            // 检查速度是否稳定（速度变化很小）
            double speed_change_x = std::abs(current_linear_x_ - prev_current_linear_x_);
            double speed_change_y = std::abs(current_linear_y_ - prev_current_linear_y_);
            double speed_change_z = std::abs(current_angular_z_ - prev_current_angular_z_);
            bool speed_stable = (speed_change_x < 0.001 && speed_change_y < 0.001 && speed_change_z < 0.001);
            
            // 如果速度已经达到目标且稳定，说明按键已松开（因为如果按键还在按着，用户会再次按下方向键）
            // 立即将目标速度归零，让速度开始减速
            if ((target_linear_x_ != 0.0 || target_linear_y_ != 0.0 || target_angular_z_ != 0.0) &&
                reached_target && speed_stable) {
                // 按键已松开，目标速度归零（当前速度会继续减速到0）
                target_linear_x_ = 0.0;
                target_linear_y_ = 0.0;
                target_angular_z_ = 0.0;
                key_pressed_ = false;
                last_key_press_time_ = std::chrono::steady_clock::time_point{};
            }
        }
    }
    
    // 直接设置当前速度为目标速度（不做递增递减处理）
    current_linear_x_ = target_linear_x_;
    current_linear_y_ = target_linear_y_;
    current_angular_z_ = target_angular_z_;
    
    // 限制速度范围
    current_linear_x_ = std::max(-max_linear_x_, std::min(max_linear_x_, current_linear_x_));
    current_linear_y_ = std::max(-max_linear_y_, std::min(max_linear_y_, current_linear_y_));
    current_angular_z_ = std::max(-max_angular_z_, std::min(max_angular_z_, current_angular_z_));
    
    // 发布速度命令
    auto twist = geometry_msgs::msg::Twist();
    twist.linear.x = current_linear_x_;
    twist.linear.y = current_linear_y_;
    twist.linear.z = 0.0;
    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    twist.angular.z = current_angular_z_;
    
    publisher_->publish(twist);
    
    // 更新上次速度（用于检测速度变化率）
    prev_current_linear_x_ = current_linear_x_;
    prev_current_linear_y_ = current_linear_y_;
    prev_current_angular_z_ = current_angular_z_;
    
    // 更新状态显示
    printStatus();
}

void ChassisController::stop() {
    // 直接置零目标速度和当前速度
    target_linear_x_ = 0.0;
    target_linear_y_ = 0.0;
    target_angular_z_ = 0.0;
    current_linear_x_ = 0.0;
    current_linear_y_ = 0.0;
    current_angular_z_ = 0.0;
    
    // 立即发布零速度命令
    auto twist = geometry_msgs::msg::Twist();
    twist.linear.x = 0.0;
    twist.linear.y = 0.0;
    twist.linear.z = 0.0;
    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    twist.angular.z = 0.0;
    publisher_->publish(twist);
    
    printStatus();
}

void ChassisController::shutdown() {
    running = false;
    stop();
    
    // 等待速度归零
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    
    std::cout << "\r\n" << std::string(70, '=') << "\r\n";
    std::cout << "控制器已关闭\r\n";
    std::cout << std::string(70, '=') << "\r\n";
    std::cout.flush();
}

KeyType ChassisController::getKey() {
    fd_set readfds;
    struct timeval timeout;
    
    FD_ZERO(&readfds);
    FD_SET(stdin_fd_, &readfds);
    timeout.tv_sec = 0;
    timeout.tv_usec = 0;
    
    if (select(stdin_fd_ + 1, &readfds, nullptr, nullptr, &timeout) > 0) {
        if (FD_ISSET(stdin_fd_, &readfds)) {
            // 先读取第一个字符
            char first_char;
            if (read(stdin_fd_, &first_char, 1) == 1) {
                // 检测方向键转义序列（\x1b[A, \x1b[B, \x1b[C, \x1b[D）
                if (first_char == '\x1b' || first_char == '\033') {
                    // 等待并读取完整的转义序列
                    // 给足够的时间让完整序列到达
                    timeout.tv_sec = 0;
                    timeout.tv_usec = 100000; // 100ms，确保完整序列到达
                    
                    char sequence[3] = {first_char, 0, 0};
                    int seq_len = 1;
                    
                    // 读取第二个字符 '['
                    FD_ZERO(&readfds);
                    FD_SET(stdin_fd_, &readfds);
                    if (select(stdin_fd_ + 1, &readfds, nullptr, nullptr, &timeout) > 0) {
                        if (read(stdin_fd_, &sequence[1], 1) == 1) {
                            seq_len = 2;
                            
                            // 读取第三个字符（方向键标识）
                            FD_ZERO(&readfds);
                            FD_SET(stdin_fd_, &readfds);
                            timeout.tv_usec = 100000; // 100ms
                            if (select(stdin_fd_ + 1, &readfds, nullptr, nullptr, &timeout) > 0) {
                                if (read(stdin_fd_, &sequence[2], 1) == 1) {
                                    seq_len = 3;
                                }
                            }
                        }
                    }
                    
                    // 解析完整的转义序列
                    if (seq_len == 3 && sequence[1] == '[') {
                        KeyType result = KeyType::NONE;
                        if (sequence[2] == 'A') result = KeyType::UP;
                        else if (sequence[2] == 'B') result = KeyType::DOWN;
                        else if (sequence[2] == 'C') result = KeyType::RIGHT;
                        else if (sequence[2] == 'D') result = KeyType::LEFT;
                        
                        if (result != KeyType::NONE) {
                            // 清空输入缓冲区中所有剩余数据
                            // 先使用 tcflush 清空
                            tcflush(stdin_fd_, TCIFLUSH);
                            // 再读取并丢弃所有剩余数据，确保完全清空
                            char discard[100];
                            timeout.tv_sec = 0;
                            timeout.tv_usec = 0;
                            while (true) {
                                FD_ZERO(&readfds);
                                FD_SET(stdin_fd_, &readfds);
                                if (select(stdin_fd_ + 1, &readfds, nullptr, nullptr, &timeout) > 0) {
                                    if (read(stdin_fd_, discard, sizeof(discard)) <= 0) break;
                                } else {
                                    break;
                                }
                            }
                            return result;
                        }
                    }
                    
                    // 如果序列不完整，清空缓冲区并返回NONE
                    // 避免不完整的序列被误判
                    tcflush(stdin_fd_, TCIFLUSH);
                    return KeyType::NONE;
                }
                
                // 处理单个字符按键
                KeyType result = KeyType::NONE;
                if (first_char == ' ') result = KeyType::SPACE;
                else if (first_char == 'x' || first_char == 'X') result = KeyType::X;
                else if (first_char == '\x03') result = KeyType::CTRL_C;
                else if (first_char == 'w' || first_char == 'W') result = KeyType::W;
                else if (first_char == 's' || first_char == 'S') result = KeyType::S;
                else if (first_char == 'a' || first_char == 'A') result = KeyType::A;
                else if (first_char == 'd' || first_char == 'D') result = KeyType::D;
                else if (first_char == 'q' || first_char == 'Q') result = KeyType::Q;
                else if (first_char == 'e' || first_char == 'E') result = KeyType::E;
                
                if (result != KeyType::NONE) {
                    // 清空输入缓冲区中所有剩余数据
                    tcflush(stdin_fd_, TCIFLUSH);
                    // 再读取并丢弃所有剩余数据，确保完全清空
                    char discard[100];
                    timeout.tv_sec = 0;
                    timeout.tv_usec = 0;
                    while (true) {
                        FD_ZERO(&readfds);
                        FD_SET(stdin_fd_, &readfds);
                        if (select(stdin_fd_ + 1, &readfds, nullptr, nullptr, &timeout) > 0) {
                            if (read(stdin_fd_, discard, sizeof(discard)) <= 0) break;
                        } else {
                            break;
                        }
                    }
                    return result;
                }
                
                // 如果是无法识别的字符，清空缓冲区
                tcflush(stdin_fd_, TCIFLUSH);
            }
        }
    }
    return KeyType::NONE;
}

void ChassisController::processKey(KeyType key) {
    switch (key) {
        case KeyType::UP:
            // 更新最后一次按键时间和状态
            last_key_press_time_ = std::chrono::steady_clock::now();
            key_pressed_ = true;
            target_linear_x_ = max_linear_x_;
            max_target_linear_x_ = max_linear_x_; // 更新最高速度
            last_target_linear_x_ = -999.0; // 强制触发显示更新
            printStatus();
            break;
        case KeyType::DOWN:
            last_key_press_time_ = std::chrono::steady_clock::now();
            key_pressed_ = true;
            target_linear_x_ = -max_linear_x_;
            max_target_linear_x_ = -max_linear_x_; // 更新最高速度
            last_target_linear_x_ = -999.0;
            printStatus();
            break;
        case KeyType::LEFT:
            last_key_press_time_ = std::chrono::steady_clock::now();
            key_pressed_ = true;
            target_angular_z_ = max_angular_z_;  // 左键改为正值（修正方向）
            max_target_angular_z_ = max_angular_z_; // 更新最高速度
            last_target_angular_z_ = -999.0;
            printStatus();
            break;
        case KeyType::RIGHT:
            last_key_press_time_ = std::chrono::steady_clock::now();
            key_pressed_ = true;
            target_angular_z_ = -max_angular_z_;  // 右键改为负值（修正方向）
            max_target_angular_z_ = -max_angular_z_; // 更新最高速度
            last_target_angular_z_ = -999.0;
            printStatus();
            break;
        case KeyType::SPACE:
        case KeyType::X:
            key_pressed_ = false;
            stop();
            last_key_press_time_ = std::chrono::steady_clock::time_point{};
            break;
        case KeyType::W:
            // 增加 x 轴向的最大速度
            max_linear_x_ += 0.01;
            max_linear_x_ = std::min(max_linear_x_, 1.0); // 限制最大值为1.0 m/s
            last_target_linear_x_ = -999.0; // 强制触发显示更新
            printStatus();
            break;
        case KeyType::S:
            // 减小 x 轴向的最大速度
            max_linear_x_ -= 0.01;
            max_linear_x_ = std::max(max_linear_x_, 0.01); // 限制最小值为0.01 m/s
            last_target_linear_x_ = -999.0;
            printStatus();
            break;
        case KeyType::A:
            // 增加旋转最高速度
            max_angular_z_ += 0.01;
            max_angular_z_ = std::min(max_angular_z_, 2.0); // 限制最大值为2.0 rad/s
            last_target_angular_z_ = -999.0;
            printStatus();
            break;
        case KeyType::D:
            // 减小旋转最高速度
            max_angular_z_ -= 0.01;
            max_angular_z_ = std::max(max_angular_z_, 0.01); // 限制最小值为0.01 rad/s
            last_target_angular_z_ = -999.0;
            printStatus();
            break;
        case KeyType::Q:
            // 增加 y 轴向最高速度
            max_linear_y_ += 0.01;
            max_linear_y_ = std::min(max_linear_y_, 1.0); // 限制最大值为1.0 m/s
            last_target_linear_y_ = -999.0;
            printStatus();
            break;
        case KeyType::E:
            // 减小 y 轴向最高速度
            max_linear_y_ -= 0.01;
            max_linear_y_ = std::max(max_linear_y_, 0.01); // 限制最小值为0.01 m/s
            last_target_linear_y_ = -999.0;
            printStatus();
            break;
        default:
            break;
    }
}

int main(int argc, char * argv[]) {
    // 设置环境变量，禁用ROS2日志输出
    setenv("RCUTILS_LOGGING_SEVERITY", "FATAL", 1);
    
    rclcpp::init(argc, argv);
    
    auto controller = std::make_shared<ChassisController>();
    
    try {
        while (controller->running && rclcpp::ok()) {
            KeyType key = controller->getKey();
            
            if (key != KeyType::NONE) {
                // 防抖处理：忽略短时间内重复的相同按键
                auto current_time = std::chrono::steady_clock::now();
                bool should_process = true;
                
                if (key == controller->last_processed_key_) {
                    auto time_since_last = std::chrono::duration<double>(
                        current_time - controller->last_processed_key_time_).count();
                    // 如果距离上次处理相同按键的时间小于防抖时间，忽略此次按键
                    if (time_since_last < controller->key_debounce_time_) {
                        should_process = false;
                    }
                }
                
                if (should_process) {
                    // std::cout << "key: " << static_cast<int>(key) << "\r\n" << std::endl;
                    if (key == KeyType::CTRL_C) {
                        break;
                    }
                    controller->processKey(key);
                    
                    // 更新防抖状态
                    controller->last_processed_key_ = key;
                    controller->last_processed_key_time_ = current_time;
                }
            } else {
                // 按键松开：如果之前有按键，现在变为NONE，打印0表示松开
                if (controller->last_processed_key_ != KeyType::NONE) {
                    // std::cout << "key: 0\r\n" << std::endl;
                    controller->last_processed_key_ = KeyType::NONE;
                }
            }
            
            rclcpp::spin_some(controller);
            
            // 添加1ms延迟，降低CPU占用
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    } catch (...) {
        // 忽略异常
    }
    
    controller->shutdown();
    rclcpp::shutdown();
    
    return 0;
}

